solver_eigen.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// wolf includes
#include "core/solver/solver_manager.h"
 
// Eigen includes
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <Eigen/SparseCholesky>
#include <Eigen/OrderingMethods>
 
#include <memory>
 
namespace wolf
{
WOLF_PTR_TYPEDEFS(SolverEigen);
 
class SolverEigen : public SolverManager
{
  public:
    enum class LinearSolverType
    {
        QR,
        CHOLESKY
    };
 
  protected:
    // problem structure (column and row indexes)
    std::map<StateBlockPtr, unsigned int>                sb_2_col_;   
    std::map<FactorBasePtr, unsigned int>                fac_2_row_;  
    std::map<StateBlockPtr, LocalParametrizationBasePtr> sb_2_local_param_;    
    std::set<StateBlockPtr>                              fixed_state_blocks_;  
    bool                                                 structure_dirty_;
    unsigned int                                         n_rows_, n_cols_;
 
    // linearized problem
    Eigen::SparseMatrix<double> A_;  
    Eigen::VectorXd             b_;  
    double                      cost_;
 
    // linear solvers
    LinearSolverType linear_solver_;
    bool             analysis_dirty_;  
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> qr_;  
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>                        ldlt_;
    struct SuiteSparseSolvers;                         
    std::unique_ptr<SuiteSparseSolvers> suitesparse_;  
    Eigen::SparseMatrix<double> R_, Rt_;  
    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> R_perm_;  
 
    // params
    unsigned int max_num_iterations_;
    double       function_tolerance_, parameter_tolerance_, gradient_tolerance_;
    bool         interrupt_on_problem_change_;  
    unsigned int min_num_iterations_;           
 
    // summary of the last solve
    unsigned int iterations_;
    double       initial_cost_, final_cost_, total_time_;
    bool         converged_, failed_, stopped_;
    std::string  termination_message_;
    unsigned int successful_steps_, unsuccessful_steps_;  
    unsigned int line_search_steps_;                      
    unsigned int n_linearizations_, n_linear_solves_;
    double       preprocessor_time_, minimizer_time_, postprocessor_time_;
    double       linearization_time_, linear_solver_time_;
 
    // profiling
    unsigned int n_iter_acc_, n_iter_max_;
    unsigned int n_convergence_, n_interrupted_, n_failed_, n_no_convergence_;
 
  public:
    SolverEigen(const ProblemPtr& _wolf_problem, const YAML::Node& _params);
 
    WOLF_SOLVER_CREATE(SolverEigen);
 
    ~SolverEigen() override;
 
    bool computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks) override;
    bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
 
    bool         converged() const override;
    bool         failed() const override;
    bool         wasStopped() const override;
    unsigned int iterations() const override;
    double       initialCost() const override;
    double       finalCost() const override;
    double       totalTime() const override;
 
    int numStateBlocksEstimatedDerived() const override;
    int sizeStateBlocksEstimatedDerived() const override;
    int numFactorsDerived() const override;
    int sizeFactorsDerived() const override;
 
    LinearSolverType getLinearSolverType() const;
 
    std::string getLinearSolverName() const;
 
    const Eigen::SparseMatrix<double>& getJacobian() const;
 
    std::string terminationType() const;
 
    std::string briefReport() const;
 
    std::string fullReport() const;
 
  protected:
    std::string solveDerived(const ReportVerbosity report_level) override;
 
    void addFactorDerived(const FactorBasePtr& fac_ptr) override;
    void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
    void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
    void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
    void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
    void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
 
    bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
    bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
    bool isStateBlockFixedDerived(const StateBlockPtr& st) const override;
    bool hasThisLocalParametrizationDerived(const StateBlockPtr&               st,
                                            const LocalParametrizationBasePtr& local_param) const override;
    bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
 
    void printProfilingDerived(std::ostream& _stream) const override;
    bool checkDerived(std::string prefix = "") const override;
 
    void reindex();
 
    bool linearize(bool _apply_loss);
 
    bool factorize();
 
    Eigen::VectorXd solveIncrement() const;
 
    Eigen::MatrixXd solveNormalEquations(const Eigen::MatrixXd& _E) const;
 
    bool computeCovarianceBlocks(const std::vector<std::pair<StateBlockPtr, StateBlockPtr>>& _pairs);
 
    void applyIncrement(const Eigen::VectorXd& _dx);
};
 
inline SolverEigen::LinearSolverType SolverEigen::getLinearSolverType() const
{
    return linear_solver_;
}
 
inline const Eigen::SparseMatrix<double>& SolverEigen::getJacobian() const
{
    return A_;
}
 
}  // namespace wolf